// **************************** wheel.c ******************************

// This file contains the interface functions for the wheel of the 
// Roboto Robusto!

#include "inc/hw_types.h"
#include "lm3s8962.h"
#include "inc/hw_memmap.h"
#include "inc/hw_gpio.h"
#include "inc/hw_ints.h"
#include "driverlib/timer.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/interrupt.h"
#include "wheel.h"
#include "ports.h"

//*Setup Port
#define INPUT_PORT_BASE                GPIO_PORTB_BASE      // ** Change according to port used **
#define INTERRUPT_VECTOR               INT_GPIOB            // **                               **
#define PORT_PERIPH_ENABLE             SYSCTL_PERIPH_GPIOB  // **                               **
#define INPUT_PIN                      GPIO_PIN_3           // ***********************************
#define INT_PRIORITY                   1                    //
//*Setup Timer                                              //
#define TIMER_BASE                     TIMER2_BASE          // 
#define TIMER_PERIPH_ENABLE            SYSCTL_PERIPH_TIMER2 //
#define TIMER_LETTER                   TIMER_BOTH           //  
#define TIMER_PRESCALE                 0xFF                 // 335.5443ms until roll over
#define TIMER_CONFIG                   TIMER_CFG_16_BIT_PAIR | TIMER_CFG_A_PERIODIC | TIMER_CFG_B_PERIODIC //
#define TIMER_CTL_REG                  TIMER2_CTL_R         //
#define TIMER_INT_A                    INT_TIMER2A          //
#define TIMER_INT_B                    INT_TIMER2B          //
#define TIMER_INT_CONFIG               (TIMER_TIMA_TIMEOUT | TIMER_TIMB_TIMEOUT) //
#define TIMER_COUNT_REG                TIMER2_TAR_R         //
#define TIMER_LOAD_COUNT_A             48828/4              // Interrupt 4 times per second
#define TIMER_LOAD_COUNT_B			   488
//*Input Capture Timers                                     //
#define IC_TIMER_RELOAD                65535                // 
#define IC_TIMER_BASE_A                TIMER0_BASE          //
#define IC_TIMER_BASE_B                TIMER1_BASE          //
#define IC_TIMER_RESULT_A              TIMER0_TBR_R         // Read to get time
#define IC_TIMER_RESULT_B              TIMER1_TBR_R         // Read to get time
#define IC_TIMER_PERIPH_ENABLE_A       SYSCTL_PERIPH_TIMER0 //
#define IC_TIMER_PERIPH_ENABLE_B       SYSCTL_PERIPH_TIMER1 //
#define IC_GPIO_A                      GPIO_PA6_CCP1        //
#define IC_GPIO_B                      GPIO_PD4_CCP3        //
#define IC_PORT_BASE_A                 GPIO_PORTA_BASE      //
#define IC_PORT_BASE_B                 GPIO_PORTD_BASE      //
#define IC_INPUT_PIN_A                 GPIO_PIN_6           //
#define IC_INPUT_PIN_B                 GPIO_PIN_4           //
#define IC_PORT_PERIPH_ENABLE_A        SYSCTL_PERIPH_GPIOA  //
#define IC_PORT_PERIPH_ENABLE_B        SYSCTL_PERIPH_GPIOD  //
#define IC_TIMER_INT_VECT_A            INT_TIMER0B          //
#define IC_TIMER_INT_VECT_B            INT_TIMER1B          //
#define IC_TIMER_WRAP_A                0x0100
#define IC_TIMER_CAP_A                 0x0400
#define IC_TIMER_WRAP_CAP_A            0x0500
#define IC_TIMER_WRAP_B                0x0100
#define IC_TIMER_CAP_B                 0x0400
#define IC_TIMER_WRAP_CAP_B            0x0500
//*Other defines                                            //
#define TICK_TIMES_PER_SEC             25000000             // 25MHz   
#define WHEEL_DEBUG                    0                    //
#define TRUE                           1                    //
#define FALSE                          0                    //
#define INTS_PER_REV                   4                    // Number of holes on the wheel

// ************************************************************
// ******************** GLOBALS ***************************
// ***************************************************
// ********* LEFT WHEEL ************
/*static*/ volatile unsigned long  TachPeriod_L = 50000; // in units of SYSTEM_TIME
/*static*/ volatile unsigned long  DesiredSpeed_L = 50000; // in units of SYSTEM_TIME
/*static*/ volatile unsigned long  LastTime_L = 0, ThisTime_L = 0;
/*static*/ volatile unsigned long  TachBuffer_L[3];
/*static*/ volatile unsigned long  TachBufferIndex_L = 0;
/*static*/ volatile long           Accel_L = 0;
/*static*/ volatile unsigned long  WheelMoving_L = 0;
/*static*/ volatile unsigned long  WheelStopped_L = TRUE;
/*static*/ volatile unsigned long  RWRevs_L = 0; // Counts down to execute precise movement
/*static*/ volatile unsigned long  LWRevs_L = 0; // 
// ********* RIGHT WHEEL ***********
/*static*/ volatile unsigned long  TachPeriod_R = 50000; // in units of SYSTEM_TIME
/*static*/ volatile unsigned long  DesiredSpeed_R = 50000; // in units of SYSTEM_TIME
/*static*/ volatile unsigned long  LastTime_R = 0, ThisTime_R = 0;
/*static*/ volatile unsigned long  TachBuffer_R[3];
/*static*/ volatile unsigned long  TachBufferIndex_R = 0;
/*static*/ volatile long           Accel_R = 0;
/*static*/ volatile unsigned long  WheelMoving_R = 0;
/*static*/ volatile unsigned long  WheelStopped_R = TRUE;
/*static*/ volatile unsigned long  Revs_L = 0; // Counts down to execute precise movement
/*static*/ volatile unsigned long  Revs_R = 0; // 
/*static*/ volatile unsigned long  Status, Status1, Status2;
/*static*/ volatile unsigned long  Wraps_B = 0;
#if WHEEL_DEBUG
unsigned long DebugDump[256];
unsigned char DumpIndex = 0;
unsigned long Max = 0, Min = 100000;
#endif 
// ***************************************************
// ********************************************************

// ***************** Wheel_Interrupt ***********************
//
// At 200 RPM this interrupt should occure every 75ms when 
// a hole passes the sensor. This interrupt routine will
// measure the time difference between the holes on the wheel
// and run a median filter on these values. It also sets a flag 
// which will indicate to other routines that the wheels are
// moving.
//
// *********************************************************
void Wheel_Interrupt_R(void){ 
  unsigned long intStatus, i, j, k, temp, period;
  static unsigned long wraps;

    // Get masked interrupt status
  intStatus = IC_TIMER_WRAP_CAP_B & TimerIntStatus(IC_TIMER_BASE_B,TRUE);
  TimerIntClear (IC_TIMER_BASE_B, intStatus);
  if( intStatus == IC_TIMER_WRAP_B || intStatus == IC_TIMER_WRAP_CAP_B ){
    wraps++;
  }
  if ( intStatus == IC_TIMER_CAP_B || intStatus == IC_TIMER_WRAP_CAP_B ) {
    // Count revolutions
  Revs_R++;
    // Difference in this call from last call in 20ns units 		
  WheelMoving_R = TRUE;
  LastTime_R = ThisTime_R;
  ThisTime_R = IC_TIMER_RESULT_B; 
  period = ((IC_TIMER_RELOAD-ThisTime_R)+LastTime_R+(wraps-1)*IC_TIMER_RELOAD);
  TachBuffer_R[TachBufferIndex_R++] = (15*TICK_TIMES_PER_SEC/period); // = RPM
  wraps = 0;	

  TachBuffer_R[TachBufferIndex_R++] = (LastTime_R-ThisTime_R); // Timer counts down  

  if( TachBufferIndex_R > 2 ){ TachBufferIndex_R = 0; }
    // Perform a median filter on the Tach Period Buffer
  i = 0; j = 1; k = 2;
     // Check i and j
  if(TachBuffer_R[i] > TachBuffer_R[j]){temp=j; j=i; i=temp;} 
    // Check j and k  
  if(TachBuffer_R[j] > TachBuffer_R[k]){temp=k; k=j; j=temp;} 
    // Check i and j (j possibly changed last step)  
  if(TachBuffer_R[i] > TachBuffer_R[j]){temp=j; j=i; i=temp;}   
    // j is the mdian value
  Accel_R = TachBuffer_R[j] - TachPeriod_R; // TachBuffer[j] = current speed, TachPeriod = last speed
  TachPeriod_R = TachBuffer_R[j]; 
  }
}

void Wheel_Interrupt_L(void){
  unsigned long intStatus, i, j, k, temp, period;
  static unsigned long wraps;

    // Get masked interrupt status
  intStatus = IC_TIMER_WRAP_CAP_A & TimerIntStatus(IC_TIMER_BASE_A,TRUE);
  TimerIntClear (IC_TIMER_BASE_A, intStatus);
  if( intStatus == IC_TIMER_WRAP_A || intStatus == IC_TIMER_WRAP_CAP_A ){
    wraps++;
  }
  if ( intStatus == IC_TIMER_CAP_A || intStatus == IC_TIMER_WRAP_CAP_A ) {
    // Count revolutions
  Revs_L++;
    // Difference in this call from last call in 20ns units 		
  WheelMoving_L = TRUE;
  LastTime_L = ThisTime_L;
  ThisTime_L = IC_TIMER_RESULT_A; 
  period = ((IC_TIMER_RELOAD-ThisTime_L)+LastTime_L+(wraps-1)*IC_TIMER_RELOAD);
  TachBuffer_L[TachBufferIndex_L++] = (15*TICK_TIMES_PER_SEC/period); // = RPM
  wraps = 0;
  
  if( LastTime_L > ThisTime_L){ // Check for timer rollover
    TachBuffer_L[TachBufferIndex_L++] = LastTime_L - ThisTime_L; // Timer counts down
  } else { // Rollover occured
    TachBuffer_L[TachBufferIndex_L++] = (IC_TIMER_RELOAD - ThisTime_L) + LastTime_L;	
  }
  
  if( TachBufferIndex_L > 2 ){ TachBufferIndex_L= 0; }
    // Perform a median filter on the Tach Period Buffer
  i = 0; j = 1; k = 2;
     // Check i and j
  if(TachBuffer_L[i] > TachBuffer_L[j]){temp=j; j=i; i=temp;} 
    // Check j and k  
  if(TachBuffer_L[j] > TachBuffer_L[k]){temp=k; k=j; j=temp;} 
    // Check i and j (j possibly changed last step)  
  if(TachBuffer_L[i] > TachBuffer_L[j]){temp=j; j=i; i=temp;}   
    // j is the mdian value
  Accel_L = TachBuffer_L[j] - TachPeriod_L; // TachBuffer[j] = current speed, TachPeriod = last speed
  TachPeriod_L = TachBuffer_L[j];
  } 
}
// ************* Wheel_CheckStop_Interrupt *****************
// 
//  This is executed several times a second to check if the 
//  wheel has stopped.
//
// *********************************************************
void Wheel_CheckStop_Interrupt(void) {
  unsigned long intStatus;
    // LED blink
  GPIO_PORTF_DATA_R ^= 0x04;
    // Get interrupt staus bit
  intStatus = TimerIntStatus(TIMER_BASE, TRUE);
    // Acknowledge interrupt
  TimerIntClear(TIMER_BASE, intStatus);
    // WheelMoving is set every time a hole moves past the sensor
  if(WheelMoving_L == FALSE){ WheelStopped_L = TRUE;}
  else { WheelStopped_L = FALSE;  WheelMoving_L = FALSE; }

      // WheelMoving is set every time a hole moves past the sensor
  if(WheelMoving_R == FALSE){ WheelStopped_R = TRUE;}
  else { WheelStopped_R = FALSE;  WheelMoving_R = FALSE; }
}

void Wheel_ChangeSpeed(unsigned long lTach, unsigned long rTach){
  if (lTach == 0xffffffff) lTach = 0;
  if (rTach == 0xffffffff) rTach = 0;
  DesiredSpeed_L = lTach;
  DesiredSpeed_R = rTach;
}

// ****************** Wheel_Speed *****************************
//
// Formats the wheel speed for 12bit packet data
//
// ************************************************************
unsigned short Wheel_Speed(void){
  return ( (TachPeriod_L << 16) & (TachPeriod_R) );
}

// ****************** Wheel_Rev_n *******************************
//
// Returns the number of quarter wheel revolutions 
//
// **************************************************************
unsigned long Wheel_Rev_L( void ){
  return Revs_L;
}
unsigned long Wheel_Rev_R( void ){
  return Revs_R;
}

// ************ Wheel_Init **************************************
//
// Initializes input port and its interrupt 
//
// ************************************************************
void Wheel_Init(void){
  unsigned long trash;
  
  #if WHEEL_DEBUG 
  Ports_Init_DigOut();
  #endif
 
//*Input Capture
    // Enable General Purpose Timer
  SysCtlPeripheralEnable(IC_TIMER_PERIPH_ENABLE_A);
  SysCtlPeripheralEnable(IC_TIMER_PERIPH_ENABLE_B);
    // Disable timer 2 before making changes
  TimerDisable(IC_TIMER_BASE_A, TIMER_B);
  TimerDisable(IC_TIMER_BASE_B, TIMER_B);
    // Configure Timer 2 as a 32 bit periodic timer
    // Timer A and B are input capture timers
  TimerConfigure(IC_TIMER_BASE_A,(TIMER_CFG_16_BIT_PAIR|
				                 TIMER_CFG_B_CAP_TIME|TIMER_CFG_A_PWM));
  TimerConfigure(IC_TIMER_BASE_B,(TIMER_CFG_16_BIT_PAIR|
				                 TIMER_CFG_B_CAP_TIME|TIMER_CFG_A_PWM));
    // Timer A and B capture time @ rising edge
  TimerControlEvent(IC_TIMER_BASE_A,TIMER_B,
                      TIMER_EVENT_POS_EDGE);
  TimerControlEvent(IC_TIMER_BASE_B,TIMER_B,
                      TIMER_EVENT_POS_EDGE);
    // Timer will coun down from the value loaded
  TimerLoadSet(IC_TIMER_BASE_A, TIMER_B, IC_TIMER_RELOAD);
  TimerLoadSet(IC_TIMER_BASE_B, TIMER_B, IC_TIMER_RELOAD);
    //  Enable port
  SysCtlPeripheralEnable(IC_PORT_PERIPH_ENABLE_A);
  SysCtlPeripheralEnable(IC_PORT_PERIPH_ENABLE_B);
    //  Configure pin as input
  GPIOPinTypeGPIOInput(IC_PORT_BASE_A, IC_INPUT_PIN_A);
  GPIOPinTypeGPIOInput(IC_PORT_BASE_B, IC_INPUT_PIN_B);
    //  Configure pad as input
  GPIOPadConfigSet(IC_PORT_BASE_A, IC_INPUT_PIN_A, GPIO_STRENGTH_2MA,
                     GPIO_PIN_TYPE_STD_WPU);
  GPIOPadConfigSet(IC_PORT_BASE_B, IC_INPUT_PIN_B, GPIO_STRENGTH_2MA,
                     GPIO_PIN_TYPE_STD_WPU);
    // Configure GPIO as input capture
  GPIOPinConfigure(IC_GPIO_A|IC_GPIO_B);
    //
  GPIOPinTypeTimer(IC_PORT_BASE_A, IC_INPUT_PIN_A);
  GPIOPinTypeTimer(IC_PORT_BASE_B, IC_INPUT_PIN_B);
    // Enable timer
  TimerEnable(IC_TIMER_BASE_A,TIMER_B);
  TimerEnable(IC_TIMER_BASE_B,TIMER_B);
    // Enable Timer Interrupt
  TimerIntEnable( IC_TIMER_BASE_A, TIMER_CAPB_EVENT | TIMER_TIMB_TIMEOUT);
  TimerIntEnable( IC_TIMER_BASE_B, TIMER_CAPB_EVENT | TIMER_TIMB_TIMEOUT);  
    // Enable Interrupt with NVIC
  IntEnable(IC_TIMER_INT_VECT_A);
  IntEnable(IC_TIMER_INT_VECT_B); 
  
//*TIMER
    // Enable General Purpose Timer
  SysCtlPeripheralEnable(TIMER_PERIPH_ENABLE);
    // Disable timer 2 before making changes
  TimerDisable(TIMER_BASE, TIMER_LETTER);
    // Configure Timer 2 as a 32 bit periodic timer
  TimerConfigure ( TIMER_BASE, TIMER_CONFIG);
  TimerPrescaleSet(TIMER_BASE, TIMER_LETTER, TIMER_PRESCALE);
    // Disable TIMER2A ADC Trigger
  TIMER_CTL_REG &= ~TIMER_CTL_TAOTE;
  TIMER_CTL_REG &= ~TIMER_CTL_TBOTE;
    // Timer will coun down from the value loaded
  TimerLoadSet(TIMER_BASE, TIMER_A, TIMER_LOAD_COUNT_A);
  TimerLoadSet(TIMER_BASE, TIMER_B, TIMER_LOAD_COUNT_B);
    // Enable TIMER2A interrupt
  TimerIntEnable( TIMER_BASE, TIMER_INT_CONFIG );
    // Register TIMER2A interrupt with NVIC
  IntEnable(TIMER_INT_A);
  IntEnable(TIMER_INT_B);
    // Enable Timer 0 countdown
  TimerEnable( TIMER_BASE, TIMER_LETTER );
  IntMasterEnable();
  
//*STATUS LED
      // Enable the GPIO port that is used for the on-board LED.
    SYSCTL_RCGC2_R = SYSCTL_RCGC2_GPIOF;
      // Do a dummy read to insert a few cycles after enabling the peripheral.
    trash = SYSCTL_RCGC2_R;
      // Enable the GPIO pin for the LED (PF0).  Set the direction as output, and
      // enable the GPIO pin for digital function.
    GPIO_PORTF_DIR_R = 0x04;
    GPIO_PORTF_DEN_R = 0x04;
	  // Delay
    trash = SYSCTL_RCGC2_R;
	  // Turn off LED
	GPIO_PORTF_DATA_R &= ~0x04;
}

int Testmain( void ) {
    // I believe this is 25MHz
  SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
                 SYSCTL_XTAL_8MHZ);
  Wheel_Init();
  while(1);
}


